
#include "MarkerMotion.h"

namespace MMM
{

MarkerMotion::MarkerMotion(unsigned int frames)
{
	setNumFrames(frames);
}

unsigned int MarkerMotion::getNumFrames()
{
	return (unsigned int)markerTrajectory.size();
}

size_t MarkerMotion::getNumMarkers()
{
	return markerLabels.size();
}

void MarkerMotion::setNumFrames(size_t frames)
{
	if (frames==0)
	{
		markerTrajectory.clear();
		return;
	}
	markerTrajectory.resize(frames);

	// init data structures
	for (unsigned int i=0;i<frames;i++)
	{
		markerTrajectory[i].reset(new LegacyMarkerData());
	}
}

std::string MarkerMotion::getMarkerLabel(size_t i)
{
	if (i>=markerLabels.size())
	{
		MMM_ERROR << "MarkerTrajectory OutOfBounds Error, Marker " << i << endl;
		return std::string();
	}
	return markerLabels[i];
}

void MarkerMotion::setMarkerLabels(std::vector< std::string > &labels)
{
	markerLabels = labels;
}

std::vector< std::string > MarkerMotion::getMarkerLabels()
{
	return markerLabels;
}

LegacyMarkerDataPtr MarkerMotion::getFrame(size_t frame)
{
	if (frame>=markerTrajectory.size())
	{
		MMM_ERROR << "MarkerTrajectory OutOfBounds Error, Frame " << frame << endl;
		return LegacyMarkerDataPtr();
	}
	return markerTrajectory[frame];
}

bool MarkerMotion::hasMarkerLabel(const std::string &labelName) const
{
	return (std::find(markerLabels.begin(),markerLabels.end(),labelName) != markerLabels.end());
}

void MarkerMotion::setFrame(size_t frame, LegacyMarkerDataPtr data)
{
	if (frame>=markerTrajectory.size())
	{
		MMM_ERROR << "MarkerTrajectory OutOfBounds Error, Frame " << frame << endl;
		return;
	}
	if (!data)
	{
		MMM_ERROR << "MarkerTrajectory Null data Error" << endl;
		return;
	}
	markerTrajectory[frame] = data;
}

void MarkerMotion::appendFrame(LegacyMarkerDataPtr data)
{
	if (!data)
	{
		MMM_ERROR << "MarkerTrajectory Null data Error" << endl;
		return;
	}
	markerTrajectory.push_back(data);
}

void MarkerMotion::print()
{
	cout << "---------- MarkerMotion ----------------" << endl;
	cout << "Labels:" << endl;
	for (size_t i=0;i<markerLabels.size();i++)
		cout << " * " << i << ":" << markerLabels[i] << endl;
	cout << "MotionFrame:" << endl;
	for (size_t i=0;i<markerTrajectory.size();i++)
	{
		cout << " *** " << i << ":" << endl;
		markerTrajectory[i]->print();
	}
	cout << "---------- MarkerMotion ----------------" << endl;
}

std::string MarkerMotion::toXML()
{
	std::string tab = "\t";
	std::stringstream res;
	//res << "<? xml version='1.0' ?>" << endl;

	std::map<std::string, MotionEntryPtr>::iterator i = motionEntries.begin();
	while (i != motionEntries.end())
	{
		res << i->second->toXML();
		i++;
	}

	if (markerLabels.size()>0)
	{

		res << "<labels>" << endl;
		for (const auto & markerLabel : markerLabels)
		{
			res << tab << "<name>" << markerLabel << "</name>" << endl;
		}
		res << "</labels>" << endl;
	}

	if (markerTrajectory.size()>0)
	{
		res << "<marker-motion>" << endl;
		for (auto & i : markerTrajectory)
		{
			res << i->toXML(); 
		}
		res << "</marker-motion>" << endl;
	}

    return res.str();
}

void MarkerMotion::segment(size_t frame1, size_t frame2)
{
    if(frame1 > frame2){
        size_t tframe = frame1;
        frame1 = frame2;
        frame2 = tframe;
    }

    std::vector<LegacyMarkerDataPtr>::iterator begin = markerTrajectory.begin() + frame1;
    std::vector<LegacyMarkerDataPtr>::iterator end = markerTrajectory.begin() + frame2;

    std::vector<LegacyMarkerDataPtr> newTraj(begin, end);

    markerTrajectory = newTraj;
}


}
